# ThreadedClient.py
#
# creates a python thread for the RobotController
# and lets the main thread go to the GUI, since
# this GUI gets unhappy if it's not the main thread

import RobotCanvas
import Tkinter
import time
import threading
import random
import Queue
import math
import sys

# which robot do you want to use?
from roombaSim import *    # should change this to import roombaSim
import roomba

class GuiPart:
    """ graphics thread using Tkinter """
    
    def __init__(self, master, queue, mapname):
        """ the constructor for the GuiPart of the application
            the queue is the message-passing interface between
            the graphics thread and the RobotController thread """
        self.queue = queue
        self.master = master  
        self.rf = RobotCanvas.RobotFrame(master)  # all the graphics are here
        
        # right now, maps have to be added before robots
        self.mapname = mapname
        self.rf.canv.loadMap(self.mapname)
        
        # the robots are here
        self.odrobot = RobotCanvas.MovableCircle( self.rf.canv, colorstr='blue' )
        self.odrobot.setGlobalPose(-424242.0, -424242.0, 0.0) # start offscreen
        self.simrobot = RobotCanvas.MovableCircle( self.rf.canv, colorstr='red' )
        self.simrobot.setGlobalPose(-424242.0, -424242.0, 0.0)
        
        # any other graphical objects...
        # the range line
        self.rangeLine = RobotCanvas.MovableLine(self.rf.canv, 
            [(-10000,-10000),(-10000,-10000)], colorstr='red', width=1)
        # the goal point
        self.goalPoint = RobotCanvas.MovablePoint(self.rf.canv, cx = -10000,
             cy = -10000, pixelradius=5, colorstr='green')
             
        # the particles
        self.particles = []
        self.ranges = []
        
    def createParticles(self, N):
        """ creates N particles for display """
        P = len(self.particles)
        if N < P:
            for i in range(N,P):
                self.particles[i].setGlobalPose(-424242.0, -424242.0, 0.0)
                self.ranges[i].changeCoords([(0,0),(0,0)])
           
        if N > P:
            for i in range(P,N):
                self.particles.append( RobotCanvas.MovableCircle( self.rf.canv, colorstr='yellow' ) )
                self.particles[-1].setGlobalPose(-424242.0, -424242.0, 0.0)
                self.ranges.append( RobotCanvas.MovableLine(self.rf.canv, 
                      [(-10000,-10000),(-10000,-10000)], colorstr='red', width=1) )
        # that's it, maybe...?
        
    def setParticles(self, locations):
        """ sets the particles' locations to those held in locations """
        P = len(self.particles);
        L = len(locations)
        if P != L:
            self.createParticles(L)
        
        # Find the minimum and maximum probabilities on which to scale
        probabilities = [location[3] for location in locations]
        minProb = min(probabilities)
        maxProb = max(probabilities)
        for i in range(L):
            self.particles[i].setGlobalPose( locations[i][0], locations[i][1], locations[i][2] )
            try:
                intensity = int(255 * ((probabilities[i] - minProb) / 
                                       (maxProb - minProb)))
            except ZeroDivisionError:
                intensity = 128

            self.particles[i].setColor(intensity, intensity, intensity)
            
            self.ranges[i].setGlobalPose(locations[i][0], locations[i][1], locations[i][2])
            if len(locations[i]) < 5:
                self.ranges[i].changeCoords([(0,0),(0,0)])
            else:
                self.ranges[i].changeCoords([(0,0),(locations[i][4],0)])


    def shutdown(self):
        """ quits the graphics cleanly """
        print "Cleaning up the GUI..."
        try:
            self.rf.quit()
            self.master.destroy()
        except:
            pass
        sys.exit(0)

    def processIncoming(self):
        """
        Handle all the messages currently in the queue (if any).
        """
        self.rf.canv.redrawRemote()

        while self.queue.qsize():
            try:
                list = self.queue.get(0)
                #execute function with 0 or more parameters
                list[0](*list[1:])
            except Queue.Empty:
                pass

        self.master.after(20, self.processIncoming) # 50 hz

        
        
class ThreadedClient:
    """
    Launches the main part of the GUI and the worker thread. some of this and
    could reside in the GUI part, but putting them here
    means that you have all the thread controls in a single place.
    Note: the above comment was not written by me... (ZD)
    """
    
    # constants to represent different robot types...
    REAL_ROOMBA = 0
    SIM_ROOMBA = 1
    
    def __init__(self, robotType, mapname):
        """
        Start the GUI and the asynchronous threads. We are in the main
        (original) thread of the application, which will later be used by
        the GUI. We spawn a new thread for the worker.
        """
        # the map
        self.mapname = mapname
        
        # basic windows
        self.root = Tkinter.Tk()
        self.root.withdraw()
        self.master = Tkinter.Toplevel(master=self.root)
        
        # Create the queue that will send messages back and forth
        self.queue = Queue.Queue()
        
        # Set up the GUI part
        # the GUI thread handles drawing and user interaction...
        self.gui = GuiPart(self.master, self.queue, self.mapname)
        
        # THE ROBOT
        #
        # some constants for different robots we might be interested in...
        self.REAL_ROOMBA = ThreadedClient.REAL_ROOMBA  # too lazy to type "ThreadedClient"
        self.SIM_ROOMBA = ThreadedClient.SIM_ROOMBA    # all the time...
        #
        # which kind of robot do we want?
        #
        if (robotType == self.REAL_ROOMBA):
            self.robotType = self.REAL_ROOMBA
            #self.comportname = '/dev/tty.RooTooth-COM0-1' # this is my desktop mac (ZD)
            self.comportname = 6
            print 'Trying to connect to robot at ', self.comportname
            self.r = roomba.Roomba(self.comportname)
            self.r.toSafeMode()  # get into driving and sensing mode...
        else:
            self.robotType = self.SIM_ROOMBA
            # the simulated robot, r and the map it is in...
            self.r = roombaSim(0,self.mapname)

        # see if this helps with getting everything settled...
        time.sleep(1)
        
        # Set up the thread to do asynchronous I/O
        # More can be made if necessary
        self.sim = threading.Thread(target=self.main)  # this calls OUR main
        self.sim.setDaemon(1)      # close thread when graphics closes
        self.sim.start()           # and go...
        
        # Start the periodic call in the GUI to check if the queue contains anything
        self.gui.processIncoming()
        
    def run(self):
        """ starts handling events to the window """
        try:
            self.root.mainloop()
        except KeyboardInterrupt:
            print 'Control-c caught from GUI. Quitting...'
            self.shutdown()     # shutdown the robot
            sys.exit(0)
        
    def getWindowEvents(self):
        """ I suppose this data should come through the queue, but since it's
            read-only it might be safe to grab it directly """
        return self.gui.rf.canv.lastKeyDown, self.gui.rf.canv.lastMouseClick
        
    def getRealPose(self):
        """ get the real pose, not the odometric pose -- this won't
            be available for a real robot!! """
        if self.robotType == self.SIM_ROOMBA:
            return [self.r.gpos.x, self.r.gpos.y, math.degrees(self.r.gpos.thr)]
            
        elif self.robotType == self.REAL_ROOMBA:
            return [0,0,0]
        
    def getData(self):
        """ general-purpose data-getting routine
            should return a dictionary of stuff you want to reason about! """
            
        if self.robotType == self.SIM_ROOMBA:
            [x,y,thr], [odx,ody,odthr], bump = self.r.getData()
            # right now this automatically draws the robot...
            self.queue.put([self.gui.simrobot.setGlobalPose, x, y, thr]) # actual pose
            self.queue.put([self.gui.odrobot.setGlobalPose, odx, ody, odthr]) # odometric pose
            
        elif self.robotType == self.REAL_ROOMBA:
            # consider last time read! make sure we're not calling too often...
            # s will be of type sensorFrame
            s = self.r.readSensors(0)  # 0 == all sensor data
            [odx,ody,odthr] = self.r.getPoseCM()
            bump = [s.leftBump, s.rightBump]
            self.queue.put([self.gui.odrobot.setGlobalPose, odx, ody, odthr]) # odometric pose
            time.sleep(0.1) # can't call this too often or we will exceed the roomba's ability to provide it!
            
        #except Exception, e:
            # print e
            # print 'Exception in getData'
            # x = 0
            # y = 0
            # thr = 0.0
            # bump = [False,False]

        odthd = math.degrees(odthr)
        return [odx,ody,odthd], bump
        
    def setVels(self, cmpersec, degpersec, noise=0.0):
        """ this is about all we have a choice over! """
        if self.robotType == self.SIM_ROOMBA:
            self.r.setVels(cmpersec, degpersec, noise)
        elif self.robotType == self.REAL_ROOMBA:
            self.r.Go(cmpersec,degpersec)
        
    def getMap(self):
        """ returns the robot's map """
        if self.robotType == self.SIM_ROOMBA:
            return self.r.simmap.items
        elif self.robotType == self.REAL_ROOMBA:
            # there is no map - yet...
            return []
        
    def shutdown(self):
        if self.robotType == self.SIM_ROOMBA:
            pass
        elif self.robotType == self.REAL_ROOMBA:
            print "Shutting off the motors..."
            self.setVels(0,0)
            # wait...
            time.sleep(2)
            print "Powering off..."
            self.r.powerOff()  # you may not want this...
            print "Closing the serial port..."
            self.r.close()
            
        print "Shutting down the gui..."
        try:                # the gui may already be gone!
            self.gui.shutdown() # shutdown the gui
        except:
            pass
        
    def getRange(self, degrees, pos = None):
        """ getRange might be usefully combined into getData """
        if self.robotType == self.SIM_ROOMBA:
            d = self.r.getRange(degrees, pos)
            odx, ody, odthr = self.r.odpos.vals()
            self.queue.put([self.gui.rangeLine.setGlobalPose,odx,ody,odthr+math.radians(degrees)])
            self.queue.put([self.gui.rangeLine.changeCoords,[(0,0),(d,0)]])
        elif self.robotType == self.REAL_ROOMBA:
            d = 0
            # this needs to be implemented!
        return d
        
    def showGoal(self, goal):
        """ just plots a point on the GUI """
        self.queue.put([self.gui.goalPoint.setGlobalPose, goal[0], goal[1], 0.0])
        #self.goalPoint.setGlobalPose(goal[0], goal[1], 0.0)
                
    def main(self):
        """ placeholder for the RobotController's main method """
        print 'In ThreadedClient\'s main method'
        print 'You don\t want to be here... you want to be in a'
        print 'derived class\'s main method...!'
        
